#define gyroinit_min_roll_data -15    //设定陀螺仪roll初始最小值
#define gyroinit_max_roll_data 15     //设定陀螺仪roll初始最大值
#define gyro_min_roll_data -90        //设定陀螺仪roll向前倾斜最小值
#define gyro_max_roll_data 90         //设定陀螺仪roll向右倾斜最大值

#define gyroinit_min_pitch_data -15   //设定陀螺仪pitch初始最小值
#define gyroinit_max_pitch_data 15    //设定陀螺仪pitch初始最大值
#define gyro_min_pitch_data -90       //设定陀螺仪pitch向左倾斜最小值
#define gyro_max_pitch_data 90        //设定陀螺仪pitch向右倾斜最小值

#define servo_up_min_angle 0          //设定云台上面的舵机能转动到的最小角度
#define servo_up_max_angle 176        //设定云台上面的舵机能转动到的最大角度
#define servo_up_init_angle 90        //设定云台上面的舵机初始角度
#define servo_down_min_angle 0        //设定云台下面的转台能转动到的最小角度
#define servo_down_max_angle 180      //设定云台下面的转台能转动到的最大角度
#define servo_down_init_angle 90

//舵机参数初始化
void servo_init(){
  servo.init(); //2个舵机初始化设置
  servo.move(servo_up_init_angle, servo_down_init_angle);
}

//两个舵机同时动作函数
void servo_move(int angle1, int angle2){
  servo.move(angle1, angle2);
}

//陀螺仪角度映射到云台动作
void gimbal_action(){
  int up_servo_angle = 0;
  int down_servo_angle = 0;
  get_gyroscope();
  roll = max(gyro_min_roll_data, roll); //确保roll不小于gyro_min_roll_data
  roll = min(gyro_max_roll_data, roll); //确保roll不大于gyro_max_roll_data
  pitch = max(gyro_min_pitch_data, pitch); //确保pitch不小于 gyro_min_pitch_data
  pitch = min(gyro_max_pitch_data, pitch);  //确保pitch不大于 gyro_max_pitch_data
  // Serial.print("Roll:"); Serial.print(roll);Serial.print(",");
  // Serial.print("pitch:"); Serial.println(pitch);
  if((roll>=gyroinit_min_roll_data) && (roll<=gyroinit_max_roll_data)){
    up_servo_angle = servo_up_init_angle;
  }
  else if(roll<gyroinit_min_roll_data){
    up_servo_angle = map(
      roll, 
      gyroinit_min_roll_data, 
      gyro_min_roll_data, 
      servo_up_init_angle, 
      servo_up_min_angle);
  }
  else{
    up_servo_angle = map(
      roll, 
      gyroinit_max_roll_data, 
      gyro_max_roll_data, 
      servo_up_init_angle, 
      servo_up_max_angle);
  }

  if((pitch>=gyroinit_min_pitch_data) && (pitch<=gyroinit_max_pitch_data)){
    down_servo_angle = servo_down_init_angle;
  }
  else if(pitch<gyroinit_min_pitch_data){
    down_servo_angle = map(
      pitch, 
      gyroinit_min_pitch_data, 
      gyro_min_pitch_data, 
      servo_down_init_angle, 
      servo_down_min_angle);
  }
  else{
    down_servo_angle = map(
      pitch, 
      gyroinit_max_pitch_data, 
      gyro_max_pitch_data, 
      servo_down_init_angle, 
      servo_down_max_angle);
  }
  // Serial.print("up_servo_angle:"); Serial.print(up_servo_angle);Serial.print(",");
  // Serial.print("down_servo_angle:"); Serial.println(down_servo_angle);
  servo_move(up_servo_angle, down_servo_angle);
  delay(50);
}